#!/usr/bin/env python3

#转动角度的计算：16进制转换为十进制后，假设十进制的值为num
#则转动的角度=(num*360)/(16383*80)=9*num/32766

#需要十进制与十六进制转换的函数


#将十进制转换成16进制
def dec_to_hex_complement(dec_value, bits=32):
    if dec_value >= 0:
        return hex(dec_value)
    else:
        # 计算补码
        max_val = 1 << bits
        complement = (max_val + dec_value) % max_val
        return hex(complement)

#print(dec_to_hex_complement(-1500))

# 将16进制字符串转换为十进制整数
def hex_to_dec(hex_str, bits=32):
    dec_value = int(hex_str, 16)
    
    # 检查是否为负数的补码
    if dec_value >= (1 << (bits - 1)):
        # 计算原始的负数值
        max_val = 1 << bits
        dec_value = (max_val + dec_value) % max_val - max_val
    
    return dec_value

#十六进制转换成对应的角度
def convert_to_angle(data):
    decimal_data=hex_to_dec(data)
    reduction_ratio=80
    encoder_full_circle=16383
    total_positions=encoder_full_circle*reduction_ratio

    angle=(decimal_data/total_positions)*360
    return angle

#角度对应的编码器数值
def cal_dec(angle):
    data=32766*angle/9
    return data

def crc16(data):
    crc = 0xFFFF
    for byte in data:
        crc ^= byte
        for _ in range(8):
            if (crc & 1) != 0:
                crc = (crc >> 1) ^ 0xA001
            else:
                crc >>= 1
            crc &= 0xFFFF
    return crc

def reverse_hex_bytes(hex_str):
    # 将字符串按每两位分割，并反转顺序
    bytes_list = [hex_str[i:i+2] for i in range(0, len(hex_str), 2)]
    reversed_bytes = ''.join(reversed(bytes_list))
    return reversed_bytes

#将角度直接转换成输入电机的编码
def AngleToOrder(num,angle):
    #先计算角度对应的编码
    value=int(cal_dec(angle))
    formatted_num=[]
    #编码转化成16进制
    Hex_str=dec_to_hex_complement(value,bits=32)[2:]
    #填充成8位
    Hex_str=Hex_str.zfill(8)
    #再将对应电机的前八位补上，成为16位
    formatted_num.append(f"0{num}100044000204{Hex_str}")
    Hex_num=formatted_num[0]
    Hex_string=bytes.fromhex(str(Hex_num))
    #计算校验位
    crc_num=crc16(Hex_string)
    #校验位转换成16进制
    crc_num_hex=dec_to_hex_complement(crc_num)[2:]
    crc_num_hex=crc_num_hex.zfill(4)
    #校验位反着取
    reversed_crc_hex = reverse_hex_bytes(crc_num_hex)
    #将前16位和校验位组合起来，成为最终指令
    print(f"joint{num} to {angle}")
    print(str(Hex_num)+reversed_crc_hex)
    final_order=bytes.fromhex(str(Hex_num)+reversed_crc_hex)
    return bytearray(final_order)

#发送读取位置信息指令
def SendOrder(num):
    formatted_num=[]
    Hex_str=f"0{num}03002c0002"
    formatted_num.append(Hex_str)
    Hex_num=formatted_num[0]
    Hex_string=bytes.fromhex(str(Hex_num))
    #计算校验位
    crc_num=crc16(Hex_string)
    #校验位转换成16进制
    crc_num_hex=dec_to_hex_complement(crc_num)[2:]
    #校验位反着取
    reversed_crc_hex = reverse_hex_bytes(crc_num_hex)
    #print(str(Hex_num)+reversed_crc_hex)
    final_order=bytes.fromhex(str(Hex_num)+reversed_crc_hex)
    return bytearray(final_order)
            

#读取当前电机的编码，转换成角度
def OrderToAngle(order):
    #读取到的数据长这样：02 03 02 00 00 00 00 0A 0B
    DataPart=order[3:7]  #数据位是上面00所在的4个字节
    Data_Hex=DataPart.hex()
    Angle=round(convert_to_angle(Data_Hex))
    return Angle

#将角度直接转换成输入电机的编码
def SpeedToOrder(num,speed):
    #速度转化成16进制
    formatted_num=[]
    Hex_str=dec_to_hex_complement(speed,bits=32)[2:]
    #填充成8位
    Hex_str=Hex_str.zfill(8)
    #再将对应电机的前八位补上，成为16位
    formatted_num.append(f"0{num}100042000204{Hex_str}")
    Hex_num=formatted_num[0]
    Hex_string=bytes.fromhex(str(Hex_num))
    #print(Hex_string)
    #计算校验位
    crc_num=crc16(Hex_string)
    #校验位转换成16进制
    #crc_num_hex=dec_to_hex_complement(crc_num)[2:]
    crc_num_hex = f"{crc_num:04x}"
    
    reversed_crc_hex = reverse_hex_bytes(crc_num_hex)
    #将前16位和校验位组合起来，成为最终指令
    #print(str(Hex_num)+reversed_crc_hex)
    final_order=bytes.fromhex(str(Hex_num)+reversed_crc_hex)
    #print(final_order)
    return bytearray(final_order)


#SpeedToOrder(1,1500)
#SpeedToOrder(6,1500)

# AngleToOrder(2,0)
# AngleToOrder(3,-30)#获得3号电机转动到-30°的编码
# AngleToOrder(4,20)
#AngleToOrder(5,1)#获得5号电机转动到40°的编码

#SpeedToOrder(2,1500)#获得3号电机正转速1500的编码
#SpeedToOrder(3,-1500)#获得3号电机反转速1500的编码
#SpeedToOrder(6,0)#3号电机停转

